#include "ControllerField.h"

#include "TrackInfo.h"


using namespace TrackInfo;

ControllerField::ControllerField()
: PotentialField()
{

}

ControllerField::~ControllerField()
{
}

/**
 * Updates all the angles for the
 * ControllerField
 */
void ControllerField::update(CarState& cs)
{
	current = cs;

	// put the charges out
	for (int i = 0; i < 19; i++)
	{
		field[i].setLength(cs.getTrack(i));
		
		field[i].setCharge(cs.getTrack(i)*MAX_DISTANCE);
		
		field[i].update();
	}
	
}

/**
 * Finds the resulting angle from all angles
 * @ param eangle - The resulting angle from where the enemies is coming from
 */

const float ControllerField::getAngle(const float eangle)
{
	float angle = 0.0f;

	int pos2;
	Charge middle = this->getChargeWithHighestPotential(&pos2);

	int pos = 9;
	float potential = -999.0f;


	for(int i = 4; i < 15; i++)
	{
		Charge charge = field[i];


		if (i == pos2 || charge.getLength() < 0)
			continue;



		float length = MathUtility::cosineLawGetC(middle.getLength(), charge.getLength(), MathUtility::convertDegToRad(charge.getAngle()));

		float pot = Charge::getPotentialFromCharges(middle, charge, length);

		if (pot > potential)
		{

			potential = pot;
			pos = i;
		}

	}

	// slerp the angles between the one with highest distance
	// and the one with the highest potential
	float a = field[pos].getAngle(), b = field[pos2].getAngle();
	float length0 = field[pos].getLength();
	float length = field[pos2].getLength();
	
	float t = length0 / (length0 + length);
	
	
	angle = MathUtility::convertDegToRad(MathUtility::slerp(a, b, MathUtility::convertDegToRad(fabs(b-a)), t));

	Turns turn = TrackInfo::analyzeTurn(current);
	
	if (turn == TURN_NONE)
	{
		angle = 0.0f;
	}



	angle = -angle;

	return angle;


}


/**
 * Finds the resulting speed
 */
const float ControllerField::getSpeed()
{
	float speed = 0.0f;

	for(ChargeContainer::iterator it = field.begin(); it != field.end(); it++)
	{
		speed += ((it->second.getCharge()/MAX_DISTANCE) * it->second.getCharge()*0.5f);
	}

	return speed;
}

void ControllerField::initAngles(float* const angles)
{
	// set angles as {-90,-75,-60,-45,-30,20,15,10,5,0,5,10,15,20,30,45,60,75,90}

	float angle = -90;
	
	for (int i = 0; i < 19; i++)
	{
		this->angles[i] = angle;
		//angles[i] += angle;
		angle += 10.0f;
	}
}
